// Tut1.cpp : Defines the initialization routines for the DLL.

#include "stdafx.h"
#include "Tut1.h"
#include <math.h>
#include "include.h"

void RUNSIMUSER(double t, double delt, double *in, double *out, void ** ptrUserData, int *pnError, LPSTR szErrorMsg)
{
	double dq0[3];
	double abc[3];
	double albe[2];

	//PI (parametry jak dla regulatora "PI7" ze schematu w PSimie)
	struct _REGPI reg1;
	reg1.kp=20;
	reg1.Tp=0.05;
	reg1.wejp=0.0;
	reg1.wyjp=0.0;
		
	out[0]=pireg(&reg1, in[0], delt);
	//out[0]= fir(in);

	//abctodq(in, t, OM50, dq0);

	//dqtoabc(in, t, OM50, abc);

	//albetodq(in, t, OM50*6, dq0);
	//albetodq(in, t, -OM50*6, dq0);

	//dqtoalbe(in, t, OM50*6, albe);	
	//dqtoalbe(in, t, -OM50*6, albe);	
}

//transformacja abc do dq
void abctodq(double *xyz, double t, double omega, double dq0[3])
{
	dq0[0]= (2.0/3.0)*(xyz[0]*cos(omega*t)+ xyz[1]*cos(omega*t-2.0*PI/3.0)+ xyz[2]*cos(omega*t+ 2.0*PI/3.0));
	dq0[1]= (2.0/3.0)*(xyz[0]*sin(omega*t)+ xyz[1]*sin(omega*t-2.0*PI/3.0)+ xyz[2]*sin(omega*t+ 2.0*PI/3.0));
	dq0[2]= (1.0/3.0)*(xyz[0]+xyz[1]+xyz[2]);
}

//transformacja dq do abc
void dqtoabc(double *xyz, double t, double omega, double abc[3])
{
	abc[0]= xyz[0]*cos(omega*t)+xyz[1]*sin(omega*t)+xyz[2];
	abc[1]= xyz[0]*cos(omega*t-2.0*PI/3.0)+xyz[1]*sin(omega*t-2.0*PI/3.0)+xyz[2];
	abc[2]= xyz[0]*cos(omega*t+2.0*PI/3.0)+xyz[1]*sin(omega*t+2.0*PI/3.0)+xyz[2];
}

// transformacja alfa/beta do dq
void albetodq(double *xyz, double t, double omega, double dq0[3])
{
	dq0[0]= xyz[0]*cos(omega*t)+ xyz[1]*sin(omega*t);
	dq0[1]= -xyz[0]*sin(omega*t)+ xyz[1]*cos(omega*t);
	dq0[2]= 0.0;
}

// transformacja dq do alfa/beta
void dqtoalbe(double *xyz, double t, double omega, double albe[2])
{
	albe[0]= xyz[0]*cos(omega*t)- xyz[1]*sin(omega*t);
	albe[1]= xyz[0]*sin(omega*t)+ xyz[1]*cos(omega*t);
}
//regulator pi
//double pireg(struct _REGPI *_pi, double wej, double Ti)
//{
//	double wyj;
//
//	wyj=(_pi->kp)*(wej+(((_pi->Tp)/Ti)-1)*(_pi->wejp))+(_pi->wyjp);
//
//	(_pi->wejp)=wej;
//	(_pi->wyjp)=wyj;
//
//	return wyj;	
//}

double pireg(struct _REGPI *_pi, double wej, double Ti)
{
	double wyj;
	double ki;
	ki=(_pi->kp)/(_pi->Tp);

	wyj=(_pi->kp)*wej+ki*Ti*((_pi->wyjp)+wej);

	(_pi->wyjp)=wyj;

	return wyj;	
}

//FIR
double fir(double *in)
{
	static double buf_input[FIR_LEN];
	static int oldest= 0;
	double probka= in[0];
	buf_input[oldest]= probka;
	double output= 0;
	int i;
	for(i= 0; i< FIR_LEN; i++)
	{
		output += wag[i] * buf_input[(oldest+i) % FIR_LEN];
	}
	oldest= (oldest+1) % FIR_LEN;
	return output;
}